# main.py -- put your code here!
import pyb
from pyb import *
import time
import pyb
from pyb import Pin
from pyb import Timer
from machine import SPI, Pin
import Servo9527, MyTrig_Echo   # 舵机与超声波模块
import agv2  # 启动小车模块
import os


def Loop_run_test():
    pyb.LED(3).on()
    flag = 1
    a = 1
    while True:
        try:
            obj = Servo9527.my_Servo(1)
            obj2 = MyTrig_Echo.Trig_Echo9527()
            if flag == 1:
                a = 1
                flag = -1
            elif flag == -1:
                a = -1
                flag = 1
            angle = a * 100
            obj.Servo_angle_speed(angle)
            obj2.Echo_run()
            time.sleep(3)
        except:
            obj.__del__()
            obj2.__del__()
            pyb.LED(3).off()
            break

# 获取路径
def get_path():
    file_lise = os.listdir()
    rood = os.getcwd()
    file_rood_path = []
    print(rood)

# 避障, 循迹, 超声波避障
class my_agv():
    def __init__(self):
        # 红外避障对象
        self.M1 = Pin('X5', Pin.IN)
        self.M2 = Pin('X6', Pin.IN)
        self.M3 = Pin('X7', Pin.IN)
        self.M4 = Pin('X8', Pin.IN)
        # 点击控制 PIN  N1, N2左轮, N3, N4 右轮
        self.N1 = Pin('Y1', Pin.OUT_PP)
        self.N2 = Pin('Y2', Pin.OUT_PP)
        self.N3 = Pin('Y3', Pin.OUT_PP)
        self.N4 = Pin('Y4', Pin.OUT_PP)
        # 运行指示灯
        self.LED4 = pyb.LED(4)
        self.LED3 = pyb.LED(3)
        self.LED2 = pyb.LED(2)

        # 速度
        self.speed1 = 0.009
        self.speed2 = 0.015

    def __del__(self):
        self.LED4.off()
        self.LED3.off()
        self.LED2.off()
        self.AGV_stop()  # 电机停止工作

    # 电机停止工作
    def AGV_stop(self):
        self.N1.low()
        self.N2.low()
        self.N3.low()
        self.N4.low()

    # 智能车左转
    def M4_get_M2_M3(self):
        while True:
            self.N1.low()     # 高电平为转
            self.N2.high()      # 高电平为正装
            self.N3.low()
            self.N4.low()
            if self.M2.value() == 1 or self.M3.value() == 1:
                break
            elif self.M1.value() == 1:
                self.M1_get_M2_M3()
            time.sleep(self.speed1)  # 这里时间差越大, 速度越慢
            self.AGV_stop()
            time.sleep(self.speed2 + 0.002)

    # 智能车右转
    def M1_get_M2_M3(self):
        while True:
            self.N1.low()
            self.N2.low()
            self.N3.low()     # 高电平为反装
            self.N4.high()    # 高电平为正装
            if self.M2.value() == 1 or self.M3.value() == 1:
                break
            elif self.M4.value() == 1:
                self.M1_get_M2_M3()

            time.sleep(self.speed1)  # 这里时间差越大, 速度越慢
            self.AGV_stop()
            time.sleep(self.speed2 + 0.002)

    # 左轮
    def wheel_run_right(self):
        self.N1.low()
        self.N2.high()

    # 直行  self.speed1, self.speed2 为限速值
    def straight(self):
        count = 0
        try:
            self.wheel_run_right()
            self.N3.low()
            self.N4.high()
            time.sleep(self.speed1)  # 这里时间差越大, 速度越慢
            self.AGV_stop()
            time.sleep(self.speed2)
            count += 1
        except Exception as e:
            print(e)

    # 智能车直行
    def go_go(self):
        while True:
            self.straight()
            if (self.M1.value() == 1):
                self.M1_get_M2_M3()
                break
            elif (self.M4.value() == 1):
                self.M4_get_M2_M3()
                break

    def agv_test(self):
        self.N1.low()
        self.N2.high()
        self.N3.low()
        self.N4.high()
        time.sleep(10)
        self.AGV_stop()

    # 智能车
    def start_run(self):
        self.LED3.on()
        print("agv2限速运行版开始运行0.03")
        try:
            self.AGV_stop()  # 电机停止工作
            while True:
                if (self.M1.value() == 1):  # 检测到物体返回0。
                    self.M1_get_M2_M3()   # 右转循迹

                if (self.M4.value() == 1):  # 检测到物体返回0。
                    self.M4_get_M2_M3()  # 左转循迹

                if self.M3.value() == 1 or self.M2.value() == 1:
                    self.go_go()    # 直行

            self.LED3.off()
        except:
            self.AGV_stop()
            self.LED3.off()
            print("异常退出!!!!")


class laser():
    def __init__(self):
        self.l1 = Pin('X12', Pin.IN)
        self.l2 = Pin('X11', Pin.OUT_PP)

    def read_laser(self):
        self.l2.low()
        print(">>>>>>>>>>>>>", self.l1.value())

if __name__=='__main__':
    Loop_run_test()
    # obj2 = my_agv()
    # obj2.start_run()
    # obj = Servo9527.my_Servo(1)
    # obj.Servo_angle(0)
    # lobj = laser()
    # lobj.read_laser()



